
/*****************************************************************************************/
/*  < wifibotControl.cpp is a program for subscribing to the topics published from 
      * roswifibot
      * rosVectorNav
      * robotControl
      * Hokuyo Node
 
  This program also publishes the topic speed, in order to move the robot to cover some specific distance or turn to some point

  <For this program , you have to use keyboard to handle the robot or view status, 
  
    Like enter q: to quit program
	       f : to move the robot forward
	       p: to print the robot status  >
  
   <Copyright (C) <iroboApp - 2013>  
    
    <Authors: Anis Koubaa <akoubaa@coins-lab.org> , Yasir Javed <yasir.javed@coins-lab.org> 

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
/*****************************************************************************************/
#include "ros/ros.h"
# include "wifibotControl.h"

using namespace std;

    // define static data members at global namespace scope
    //all static data members must be defined at global scope
    ros::Publisher wifibotControl::_speedPublisher;
    ros::Subscriber wifibotControl::subscriberStatus;
    ros::Subscriber wifibotControl::subscriberOdom;
    ros::Subscriber wifibotControl::subscriberVN100;
    ros::Subscriber wifibotControl::subscriberQuan;
    ros::Subscriber wifibotControl::subscriberAccel ;
    ros::Subscriber wifibotControl::subscriberAngRate;
    ros::Subscriber wifibotControl::subscriberWifibotPosition;
    ros::Subscriber wifibotControl::subscriberImuData;
    ros::Subscriber wifibotControl::subscriberKalmanFilterData;
    ros::Subscriber wifibotControl::subscriberEkfPoseData;
    
    // Status Report
    nav_msgs::Odometry wifibotControl::wifibotPose;
    geometry_msgs::Quaternion wifibotControl::quaternionStatus;
    geometry_msgs::Vector3 wifibotControl::angularRateStatus;
    geometry_msgs::Vector3 wifibotControl::accelerationStatus;
    robotControl::speed_msg wifibotControl::topicSpeed; 
    robotControl::WStatus wifibotControl::wifibotGlobalStatus;
    robotControl::vn100_msg wifibotControl::vnGlobalStatus;
    robotControl::positionWifibot_msg wifibotControl::positionWbtStatus;
    sensor_msgs::Imu wifibotControl::messageImu;
    robotControl::KalmanFilteredPosition_msg wifibotControl::messageKalmanFiltered;
    geometry_msgs::PoseWithCovarianceStamped wifibotControl::EkfPoseData; // for EFK implemenation of ROS Navigation Stack "robot_pose_ekf"
    

    
/**********************************************************/
// Function vnKalmancallback 
// Input: sensor_msgs::Imu
//Output : Updating the IMU data
/**********************************************************/    
void wifibotControl::EkfPoseCallback(geometry_msgs::PoseWithCovarianceStamped msgEkfPose){
  EkfPoseData.header.seq = msgEkfPose.header.seq;
  EkfPoseData.header.stamp = msgEkfPose.header.stamp;
  EkfPoseData.header.frame_id = msgEkfPose.header.frame_id;
}
    
/**********************************************************/
// Function vnKalmancallback 
// Input: sensor_msgs::Imu
//Output : Updating the IMU data
/**********************************************************/    
void wifibotControl::vnKalmanCallback(robotControl::KalmanFilteredPosition_msg msgKFPOS){
  messageKalmanFiltered.x = msgKFPOS.x;
  messageKalmanFiltered.y = msgKFPOS.y;
  messageKalmanFiltered.z = msgKFPOS.z;
  messageKalmanFiltered.w = msgKFPOS.w;
  messageKalmanFiltered.bias1 = msgKFPOS.bias1;
  messageKalmanFiltered.bias2 = msgKFPOS.bias2;
  messageKalmanFiltered.bias3 = msgKFPOS.bias3; 
}
/**********************************************************/
// Function vnIMUCallback 
// Input: sensor_msgs::Imu
//Output : Updating the IMU data
/**********************************************************/    
void  wifibotControl::vnIMUCallback(sensor_msgs::Imu msgImu)
{
messageImu.linear_acceleration.x = msgImu.linear_acceleration.x;
messageImu.linear_acceleration.y = msgImu.linear_acceleration.y;
messageImu.linear_acceleration.z = msgImu.linear_acceleration.z;

messageImu.angular_velocity.x = msgImu.angular_velocity.x;
messageImu.angular_velocity.y = msgImu.angular_velocity.y;
messageImu.angular_velocity.z = msgImu.angular_velocity.z;

messageImu.orientation.x = msgImu.orientation.x;
messageImu.orientation.y = msgImu.orientation.y;
messageImu.orientation.z = msgImu.orientation.z;
messageImu.orientation.w = msgImu.orientation.w;

}    
/**********************************************************/
// Function statusCallback 
// Input: robotControl::WStatus
//Output : Printing the Status of Wifibot
/**********************************************************/
void wifibotControl::statusCallback(robotControl::WStatus msg){
  // Global Status of wifibot is updated
  wifibotGlobalStatus.battery_level  = msg.battery_level;
  wifibotGlobalStatus.current =  msg.current;
  wifibotGlobalStatus.ADC1 = msg.ADC1;
  wifibotGlobalStatus.ADC2 =  msg.ADC2;
  wifibotGlobalStatus.ADC3 = msg.ADC3;
  wifibotGlobalStatus.ADC4 = msg.ADC4;
  wifibotGlobalStatus.speed_front_left =  msg.speed_front_left;
  wifibotGlobalStatus.speed_front_right =  msg.speed_front_right;
  wifibotGlobalStatus.odometry_left =msg.odometry_left;
  wifibotGlobalStatus.odometry_right = msg.odometry_right ;
  wifibotGlobalStatus.version  =  msg.version;
}

/**********************************************************/
// Function statusCallback 
// Input: robotControl::WStatus
//Output : Printing the Status of Wifibot
/**********************************************************/
void wifibotControl::positionStatusCallback(robotControl::positionWifibot_msg msg){
        positionWbtStatus.x = msg.x;
	positionWbtStatus.y = msg.y;
	positionWbtStatus.z = msg.z;
	positionWbtStatus.theta = msg.theta;
}
/**********************************************************/
// Function vnStatusCallback
// Input: robotControl::vn100_msg
//Output : Printing the Orientation accroding to Vector Nav
/**********************************************************/
void wifibotControl::vnStatusCallback(robotControl::vn100_msg msg){
  vnGlobalStatus.yaw = msg.yaw;
  vnGlobalStatus.pitch = msg.pitch;
  vnGlobalStatus.roll = msg.roll;
}

/**********************************************************/
// Function odomCallback
// Input: const nav_msgs::Odometry odom
//Output : getting location X,Y of the robot calculated in wifibot.cpp
/**********************************************************/
void wifibotControl::odomCallback(const nav_msgs::Odometry odom){
  wifibotPose.pose.pose.position.x=odom.pose.pose.position.x;
  wifibotPose.pose.pose.position.y=odom.pose.pose.position.y; 
}
/**********************************************************/
// Function vnQuaternionCallback
// Input: geometry_msgs::Quaternion quaternionMsg
//Output : getting location X,Y,Z,W of the robot calculated in vn100.c
/**********************************************************/

void wifibotControl::vnQuaternionCallback(const geometry_msgs::Quaternion quaternionMsg){
quaternionStatus.x = quaternionMsg.x;
quaternionStatus.y = quaternionMsg.y;
quaternionStatus.z = quaternionMsg.z;
quaternionStatus.w = quaternionMsg.w;
}

/**********************************************************/
// Function vnAccelerationCallback
// Input: cgeometry_msgs::Vector3 
//Output : getting location X,Y,Z,W of the robot calculated in vn100.c
/**********************************************************/
void wifibotControl::vnAccelerationCallback(const geometry_msgs::Vector3  msg){
  accelerationStatus.x = msg.x;
  accelerationStatus.y = msg.y;
  accelerationStatus.z = msg.z;
}

/**********************************************************/
// Function vnAngularRateCallback
// Input: geometry_msgs::Vector3 
//Output : getting location X,Y,Z,W of the robot calculated in vn100.c
/**********************************************************/
void wifibotControl::vnAngularRateCallback(const geometry_msgs::Vector3  angulRateMsg){
  angularRateStatus.x = angulRateMsg.x;
  angularRateStatus.y = angulRateMsg.y;
  angularRateStatus.z = angulRateMsg.z;
}

/**********************************************************/
// Function menu
// Input: 
//Output : Printing the main options of the wifibotControl file
/**********************************************************/
void wifibotControl::menu()
{
	printf( "----------------------------------------------------\n");
	printf( "Welcome to ROS Wifibot Control Program\n \n");
	printf( "Choose an action from the menu\n \n");
	printf( "'p','P': Robot and VN Status\n");
	printf( "'o','O': Odometry Status\n");
	printf( "'v','V': VectorNav Status\n");
	printf( "'w','W': Wifibot Status\n");
	printf( "'f','F': Move Forward\n");
	printf( "'m','M': Map Navigating\n");
	printf( "'r','R': Rotate Robot\n");
	printf( "'q','Q': Quit\n");
	printf( "----------------------------------------------------\n");
}


/**********************************************************/
int wifibotControl::Move(DISTANCE distance, SPEED speed){
	
	ODOMETRY SDL_0 = wifibotGlobalStatus.odometry_left;
	ODOMETRY SDR_0 = wifibotGlobalStatus.odometry_right;

	//if the speed if positive, then move forward
	if (speed>0.0){
	    while (((wifibotGlobalStatus.odometry_left+wifibotGlobalStatus.odometry_right)/2)<((SDL_0+SDR_0)/2)+(distance)){
	    //keep publish the speed until the robot goes the specified distance
	    topicSpeed.speedLeft = speed;
	    topicSpeed.speedRight =  speed;
	    _speedPublisher.publish(topicSpeed);
	    //ros::spinOnce();
	    }
	} else 
	  if (speed<0.0){
	      while (((wifibotGlobalStatus.odometry_left+wifibotGlobalStatus.odometry_right)/2)>((SDL_0+SDR_0)/2)-(distance)){
	      //setMotorSpeed(CLOSED_LOOP_SPEED_BACKWARD+DEFAULT_SPEED, CLOSED_LOOP_SPEED_BACKWARD+DEFAULT_SPEED);
	      topicSpeed.speedLeft =speed ;
	      topicSpeed.speedRight = speed ;
	      _speedPublisher.publish(topicSpeed);
	      }
	  }
	
	return 1;
}

/**********************************************************/
int wifibotControl::Rotate(BEARING rotationAngle, SPEED speed){
	BEARING initialAngle, newangle;
	/*adjust rotation angle to be within the range -180 and 180*/
	if (rotationAngle >= 180){ 
		rotationAngle = rotationAngle - 360;}
	else 
		if (rotationAngle < -180){ 
		rotationAngle = rotationAngle + 360;}
	cout << "Rotation Angle is " << rotationAngle<<endl;
	//rotationAngle = adjustBearing (rotationAngle);
	/*read the current yaw in the range -180 and 180 degrees*/
	initialAngle = vnGlobalStatus.yaw;
	//Location.setOrientation(initialAngle);
        newangle = initialAngle + rotationAngle; 
 	cout << " THe new angle is "<< newangle <<endl;
			if (rotationAngle <0)//turn left
			{
			  cout << "Left The Yaw value : " <<vnGlobalStatus.yaw << " condition "<< newangle +  VN_YAW_OFFSET<< endl;
			if (newangle < -180) 
			while(vnGlobalStatus.yaw > newangle +  VN_YAW_OFFSET)
			{ void EkfPoseCallback(geometry_msgs::PoseWithCovarianceStamped msgEkfPose);
     
			  topicSpeed.speedLeft =  -speed;
			  topicSpeed.speedRight =  speed;
			  _speedPublisher.publish(topicSpeed);
			}
			else if (newangle > - 180)
			while(vnGlobalStatus.yaw> newangle +  VN_YAW_OFFSET)
			{
			  topicSpeed.speedLeft = -speed;
			  topicSpeed.speedRight =  speed;
			  _speedPublisher.publish(topicSpeed);
			}
			}
			else 
			if (rotationAngle >0)//turn right
			{
			 cout << " Right: The Yaw value : " <<vnGlobalStatus.yaw << " condition "<< newangle +  VN_YAW_OFFSET<< endl;
			
			if (newangle > 180)
			while(vnGlobalStatus.yaw < newangle -  VN_YAW_OFFSET)
			{
			  topicSpeed.speedLeft = speed;
			  topicSpeed.speedRight = -speed;
			  _speedPublisher.publish(topicSpeed);
			}
			else if (newangle < 180)
			while(vnGlobalStatus.yaw < newangle -  VN_YAW_OFFSET)
			{
			  topicSpeed.speedLeft = speed;
			  topicSpeed.speedRight = -speed;
			  _speedPublisher.publish(topicSpeed);
			}
			}
		return 1;
	
	
}


int wifibotControl::printWifibotStatus(){
  cout <<"*********************** WIFIBOT STATUS ***********************" <<endl;   
  cout << "wifibotGlobalStatus.battery_level:  "<< wifibotGlobalStatus.battery_level<<endl;
    cout <<"wifibotGlobalStatus.current: "<<wifibotGlobalStatus.current <<endl;
    cout <<"wifibotGlobalStatus.ADC1: " << wifibotGlobalStatus.ADC1<<endl;
    cout <<"wifibotGlobalStatus.ADC2: "<< wifibotGlobalStatus.ADC2<<endl;
    cout <<"wifibotGlobalStatus.ADC3: " << wifibotGlobalStatus.ADC3<<endl;
    cout <<"wifibotGlobalStatus.ADC4: " << wifibotGlobalStatus.ADC4<<endl;
    cout <<"wifibotGlobalStatus.speed_front_left: "  << wifibotGlobalStatus.speed_front_left<<endl;
    cout <<"wifibotGlobalStatus.speed_front_right: "  << wifibotGlobalStatus.speed_front_right<<endl;
    cout <<"wifibotGlobalStatus.odometry_left: " << wifibotGlobalStatus.odometry_left<<endl;
    cout <<"wifibotGlobalStatus.odometry_right: "<< wifibotGlobalStatus.odometry_right<<endl;
    cout <<"wifibotGlobalStatus.version: " << wifibotGlobalStatus.version<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"X Coordinate: " <<  wifibotPose.pose.pose.position.x<<endl;
    cout <<"Y Coordinate: " <<  wifibotPose.pose.pose.position.y<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"Position X: " <<  positionWbtStatus.x<<endl;
    cout <<"Position Y: " <<  positionWbtStatus.y<<endl;
    cout <<"Position Z: " <<  positionWbtStatus.z<<endl;
    cout <<"Position theta: " <<  positionWbtStatus.theta<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"Yaw Value VN: " <<  vnGlobalStatus.yaw<<endl;
    cout <<"Pitch Value VN: " <<  vnGlobalStatus.pitch<<endl;
    cout <<"Roll Value VN: " <<  vnGlobalStatus.roll<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"Quaternion q0: " << quaternionStatus.x<<endl;
    cout <<"Quaternion q1: " << quaternionStatus.y<<endl;
    cout <<"Quaternion q2: " << quaternionStatus.z<<endl;
    cout <<"Quaternion q3: " << quaternionStatus.w<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"Acceleration on X: " <<  accelerationStatus.x<<endl;
    cout <<"Acceleration on Y: " <<  accelerationStatus.y<<endl;
    cout <<"Acceleration on Z: " <<  accelerationStatus.z<<endl;
    cout <<"*************************************" <<endl; 
    cout <<"AngularRate on X: " <<  angularRateStatus.x<<endl;
    cout <<"AngularRate on Y: " <<  angularRateStatus.y<<endl;
    cout <<"AngularRate on Z: " << angularRateStatus.z<<endl;
    cout <<"************************************************************" <<endl; 
return 1;	
  
}

////////////////////////The Main function ///////////////////////////////////
int main(int argc, char **argv)
{
 
  char c='z';
  
   ros::init(argc, argv, "listener");
   ros::NodeHandle n;
   ROS_INFO("Starting wifibotControl.cpp!");
  
  wifibotControl wfc;
  //publish the speed
  wifibotControl::_speedPublisher= n.advertise<robotControl::speed_msg>("cmd_speed",1);
   //subscribe to vn100 topic published by wifibot.cpp
   wifibotControl::subscriberVN100 = n.subscribe("vn_ypr_topic", 2, &wifibotControl::vnStatusCallback, &wfc);
   //subscribe to Angular Rate topic published by wifibot.cpp
   wifibotControl::subscriberAngRate = n.subscribe("vn_angular_rate_topic", 2, &wifibotControl::vnAngularRateCallback, &wfc);
   //subscribe to Acceleration published by wifibot.cpp
   wifibotControl::subscriberAccel = n.subscribe("vn_acceleration_topic", 2, &wifibotControl::vnAccelerationCallback, &wfc);
  //subscribe to Acceleration published by wifibot.cpp
   wifibotControl::subscriberWifibotPosition = n.subscribe("wifibot_position_topic", 2, &wifibotControl::positionStatusCallback, &wfc);
   //subscribe to vectorSpaceStatus topic published by wifibot.cpp
   wifibotControl::subscriberQuan = n.subscribe("vn_quaternion_topic", 2, &wifibotControl::vnQuaternionCallback, &wfc);
   //subscribe to wifibot status topic published by wifibot.cpp
   wifibotControl::subscriberStatus = n.subscribe("status", 2, &wifibotControl::statusCallback, &wfc);
   //subscribe to wifibot odom (X,Y coordinates) topic published by wifibot.cpp
   wifibotControl::subscriberOdom = n.subscribe("odom", 2, &wifibotControl::odomCallback, &wfc);
  //subscribe to wifibot odom (X,Y coordinates) topic published by wifibot.cpp
   wifibotControl::subscriberImuData = n.subscribe("imu_data", 2, &wifibotControl::vnIMUCallback, &wfc);
  //subscribe to wifibot odom (X,Y coordinates) topic published by wifibot.cpp
   wifibotControl::subscriberKalmanFilterData = n.subscribe("kalman_filter_data", 2, &wifibotControl::vnKalmanCallback, &wfc);
     
   // AsyncSpinner 
   //imer
      ros::AsyncSpinner statusSpinner(8);
      statusSpinner.start();
      ros::Rate threadRate(10);
   //ros::spin();
   //int i=0;
   
  while (ros::ok()){ // rosok bracket
    //menu();
      DISTANCE dist=0.0;
      BEARING theta=0.0;
      SPEED spd = 0.0;
      cout <<"Enter a Character and press Enter to move. Enter q to Quit ..." <<endl;
    
	cin >> c;
	if (c=='q') return 1;
        if(c=='f'){
	    cout<< "Enter the distance to Move";
	    cin >>dist;
	    cout<< "Enter the speed: [0.2 is recommended]" ;
	    cin >>spd;
	    wifibotControl::printWifibotStatus();
	    threadRate.sleep();
	    wifibotControl::Move(dist,spd);
	    threadRate.sleep();
	    wifibotControl::printWifibotStatus();
	}
	 if(c=='r'){
	    cout<< "Enter the degree to Rotate";
	    cin >>theta;
	    cout<< "Enter the speed: [0.2 is recommended]" ;
	    cin >>spd;
	    wifibotControl::printWifibotStatus();
	    threadRate.sleep();
	    wifibotControl::Rotate(theta,spd);
	    threadRate.sleep();
	    wifibotControl::printWifibotStatus();
	}
	if(c=='p'){
	   threadRate.sleep();
	 wifibotControl::printWifibotStatus();
	}
	if(c=='l'){
	  threadRate.sleep();
	 cout <<"X Coordinate: " <<  wifibotControl::wifibotPose.pose.pose.position.x<<endl;
	 cout <<"Y Coordinate: " <<  wifibotControl::wifibotPose.pose.pose.position.y<<endl;
	}
	if(c=='s'){
	  threadRate.sleep();
	 cout <<"VectorSpace CoOrdinate X: " << wifibotControl::quaternionStatus.x<<endl;
         cout <<"VectorSpace CoOrdinate Y: " << wifibotControl::quaternionStatus.y<<endl;
         cout <<"VectorSpace CoOrdinate Z: " << wifibotControl::quaternionStatus.z<<endl;
         cout <<"VectorSpace CoOrdinate W: " << wifibotControl::quaternionStatus.w<<endl;
        }
        if(c=='a'){
	  threadRate.sleep();
	 cout <<"Acceleration on CoOrdinate X: " << wifibotControl::accelerationStatus.x<<endl;
         cout <<"Acceleration on CoOrdinate Y: " << wifibotControl::accelerationStatus.y<<endl;
         cout <<"Acceleration on CoOrdinate Z: " << wifibotControl::accelerationStatus.z<<endl;
         }
          if(c=='t'){
	  threadRate.sleep();
	 cout <<"AngularRate on CoOrdinate X: " << wifibotControl::angularRateStatus.x<<endl;
         cout <<"AngularRate on CoOrdinate Y: " << wifibotControl::angularRateStatus.y<<endl;
         cout <<"AngularRate on CoOrdinate Z: " << wifibotControl::angularRateStatus.z<<endl;
         }
        if(c=='v'){
	    threadRate.sleep();
	    cout <<"Yaw Value VN: " <<  wifibotControl::vnGlobalStatus.yaw<<endl;
	    cout <<"Pitch Value VN: " <<  wifibotControl::vnGlobalStatus.pitch<<endl;
	    cout <<"Roll Value VN: " <<  wifibotControl::vnGlobalStatus.roll<<endl;
	  }
	  if(c=='w'){
	    threadRate.sleep();
	   cout <<"Position X: " <<  wifibotControl::positionWbtStatus.x<<endl;
	   cout <<"Position Y: " <<  wifibotControl::positionWbtStatus.y<<endl;
	   cout <<"Position Z: " <<  wifibotControl::positionWbtStatus.z<<endl;
	   cout <<"Position theta: " <<  wifibotControl::positionWbtStatus.theta<<endl; 
	  }
	  if(c=='k'){
	    threadRate.sleep();
	   cout <<"Position X: " <<  wifibotControl::positionWbtStatus.x<<endl;
	   cout <<"Position Y: " <<  wifibotControl::positionWbtStatus.y<<endl;
	   cout <<"Position Z: " <<  wifibotControl::positionWbtStatus.z<<endl;
	   cout <<"Position w: " <<  wifibotControl::positionWbtStatus.theta<<endl;
	   cout <<"Gyro Bias 1: " <<  wifibotControl::positionWbtStatus.theta<<endl;
	   cout <<"Gyro Bias 2: " <<  wifibotControl::positionWbtStatus.theta<<endl;
	   cout <<"Gyro Bias 3: " <<  wifibotControl::positionWbtStatus.theta<<endl;
	    
	  }
   
    
  }
     statusSpinner.stop();
  return 0;
}

